import math

class PurePursuitControl:
    def __init__(self):
        self.k = 0.1
        self.Lfc = 0.12
        self.L = 0.145
        self.pre_ind = 0
        self.ind = 0
        self.target_speed = 0.1
        self.STOP_FLAG = False
        self.path_len = 0
        

    def pure_pursuit_control(self, current_state, path):
        self.path_len = len(path.x)
        '''
        控制算法
        '''
        if self.pre_ind == 0:
            # calc_target_index   搜索与小车当前位置最近一个点的索引
            dx = [current_state.x - i_path_x for i_path_x in path.x] # 横坐标距离数组
            dy = [current_state.y - i_path_y for i_path_y in path.y] # 纵坐标距离数组
            d = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] # 车辆当前点与path每个点的距离（list）
            self.ind = d.index(min(d)) # 找到最小距离的索引
        else:
            self.ind = self.pre_ind

        Lf = self.k * current_state.v + self.Lfc # 预瞄距离，与速度成正比        
        Ld = math.hypot(current_state.x - path.x[self.ind], current_state.y - path.y[self.ind])
        while Ld < Lf and self.ind < len(path.x) - 1:
            Ld = math.hypot(current_state.x - path.x[self.ind], current_state.y - path.y[self.ind]) # 找到self.path上与车辆位置距离大于预瞄距离的点的索引
            self.ind += 1

        cx = path.x[self.ind]
        cy = path.y[self.ind] # 计算预瞄方向与车身方向的夹角α
        alpha = math.atan2(cy - current_state.y, cx - current_state.x) - current_state.yaw
        # print("-----------> alpha = ", math.degrees(alpha))
        
        # 计算小车前轮转角δ，核心公式
        self.delta = math.atan2(2.0 * self.L * math.sin(alpha), Lf) * path.direction[self.ind]
        
        # limit_angle = 25 # 限制delta转角
        # if abs(delta) > math.radians(limit_angle):
        #     delta = math.radians(limit_angle) if delta > 0 else -math.radians(limit_angle)
        self.pre_ind = self.ind
        self.is_stop()
        
        v, w = self.control_v_w(path)
        
        return v, w

    
    def is_stop(self):
        if self.ind >= (self.path_len - 1): # 必须减1
            self.STOP_FLAG = True
    
    def control_v_w(self, path):
        v = path.direction[self.pre_ind] * self.target_speed # 无速度规划 # 正负速度
        w = self.target_speed * math.tan(self.delta) / self.L
        return v, w
    
    def find_target_point(self):
        pass
    
    def cal_distance_min(self):
        pass